import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from blend import Blend


def animate(points, trajs):
    def update(i):
        plt.cla()
        ax1.plot3D(*zip(*trajs), 'gray')
        for j in range(len(points)):
            ax1.plot3D(points[j][0], points[j][1], points[j][2], 'b', marker='o')
        ax1.plot3D(trajs[i][0], trajs[i][1], trajs[i][2], 'r', marker='o')
        ax1.set_xlabel(r'$x$')
        ax1.set_ylabel(r'$y$')
        ax1.set_zlabel(r'$z$')

    fig = plt.figure(1)
    ax1 = plt.axes(projection='3d')
    ax1.plot3D(*zip(*trajs), 'gray')
    for j in range(len(points)):
        ax1.plot3D(points[j][0], points[j][1], points[j][2], 'b', marker='o')
    ani = animation.FuncAnimation(fig=fig, func=update, frames=np.arange(0, len(trajs)), interval=10)
    ax1.set_xlabel(r'$x$')
    ax1.set_ylabel(r'$y$')
    ax1.set_zlabel(r'$z$')
    # ani.save('trajectory.gif')
    plt.show()


if __name__ == "__main__":
    point0 = (1.0, 0.0, 0.0)
    point1 = (0.0, 2.0, 0.0)
    point2 = (2.0, 2.0, 4.0)
    points = [point0, point1, point2]
    line1 = [point0, point1]
    line2 = [point1, point2]
    delta = 0.5
    blend = Blend(line1, line2, delta)
    num_points = 100
    s_values = np.linspace(0, 1, num_points)
    interpolated_points = []
    for s in s_values:
        interpolated_points.append(tuple(blend.interpolate(s)))
    animate(points, interpolated_points)
